{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "import pandas as pd\n",
    "import numpy as np\n",
    "import matplotlib.pyplot as plt\n",
    "import seaborn as sns\n",
    "from PIL import Image\n",
    "from PIL import ImageDraw\n",
    "import tensorflow as tf\n",
    "import random\n",
    "from collections import deque\n",
    "plt.style.use({'figure.figsize':(10, 10)})\n",
    "pd.set_option('max_rows', 300)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "States=np.array(['L60D0/30/H0','L180D0/30/H0','L250D0/30/H0','L300D0/30/H0',\n",
    "'L60D30/60/H0','L180D30/60/H0','L250D30/60/H0','L300D30/60/H0',\n",
    "'L60D60/90/H0','L180D60/90/H0','L250D60/90/H0','L300D60/90/H0',\n",
    "'L60D90/120/H0','L180D90/120/H0','L250D90/120/H0','L300D90/120/H0',\n",
    "'L60D120/150/H0','L180D120/150/H0','L250D120/150/H0','L300D120/150/H0',\n",
    "'L60D150/180/H0','L180D150/180/H0','L250D150/180/H0','L300D150/180/H0',\n",
    "'L60D180/210/H0','L180D180/210/H0','L250D180/210/H0','L300D180/210/H0',\n",
    "'L60D210/240/H0','L180D210/240/H0','L250D210/240/H0','L300D210/240/H0',\n",
    "'L60D240/270/H0','L180D240/270/H0','L250D240/270/H0','L300D240/270/H0',\n",
    "'L60D270/300/H0','L180D270/300/H0','L250D270/300/H0','L300D270/300/H0',\n",
    "'L60D300/330/H0','L180D300/330/H0','L250D300/330/H0','L300D300/330/H0',\n",
    "'L60D330/360/H0','L180D330/360/H0','L250D330/360/H0','L300D330/360/H0',\n",
    "'L60D0/30/H1','L180D0/30/H1','L250D0/30/H1','L300D0/30/H1',\n",
    "'L60D30/60/H1','L180D30/60/H1','L250D30/60/H1','L300D30/60/H1',\n",
    "'L60D60/90/H1','L180D60/90/H1','L250D60/90/H1','L300D60/90/H1',\n",
    "'L60D90/120/H1','L180D90/120/H1','L250D90/120/H1','L300D90/120/H1',\n",
    "'L60D120/150/H1','L180D120/150/H1','L250D120/150/H1','L300D120/150/H1',\n",
    "'L60D150/180/H1','L180D150/180/H1','L250D150/180/H1','L300D150/180/H1',\n",
    "'L60D180/210/H1','L180D180/210/H1','L250D180/210/H1','L300D180/210/H1',\n",
    "'L60D210/240/H1','L180D210/240/H1','L250D210/240/H1','L300D210/240/H1',\n",
    "'L60D240/270/H1','L180D240/270/H1','L250D240/270/H1','L300D240/270/H1',\n",
    "'L60D270/300/H1','L180D270/300/H1','L250D270/300/H1','L300D270/300/H1',\n",
    "'L60D300/330/H1','L180D300/330/H1','L250D300/330/H1','L300D300/330/H1',\n",
    "'L60D330/360/H1','L180D330/360/H1','L250D330/360/H1','L300D330/360/H1',\n",
    "'L60D0/30/H2','L180D0/30/H2','L250D0/30/H2','L300D0/30/H2',\n",
    "'L60D30/60/H2','L180D30/60/H2','L250D30/60/H2','L300D30/60/H2',\n",
    "'L60D60/90/H2','L180D60/90/H2','L250D60/90/H2','L300D60/90/H2',\n",
    "'L60D90/120/H2','L180D90/120/H2','L250D90/120/H2','L300D90/120/H2',\n",
    "'L60D120/150/H2','L180D120/150/H2','L250D120/150/H2','L300D120/150/H2',\n",
    "'L60D150/180/H2','L180D150/180/H2','L250D150/180/H2','L300D150/180/H2',\n",
    "'L60D180/210/H2','L180D180/210/H2','L250D180/210/H2','L300D180/210/H2',\n",
    "'L60D210/240/H2','L180D210/240/H2','L250D210/240/H2','L300D210/240/H2',\n",
    "'L60D240/270/H2','L180D240/270/H2','L250D240/270/H2','L300D240/270/H2',\n",
    "'L60D270/300/H2','L180D270/300/H2','L250D270/300/H2','L300D270/300/H2',\n",
    "'L60D300/330/H2','L180D300/330/H2','L250D300/330/H2','L300D300/330/H2',\n",
    "'L60D330/360/H2','L180D330/360/H2','L250D330/360/H2','L300D330/360/H2'])\n",
    "Actions=np.array(['FrontH0','FrontH1','FrontH2','Left_45D','Right_45D'])\n",
    "Length_state=len(States)\n",
    "Length_action=len(Actions)\n",
    "Action_list=np.identity(Length_action)\n",
    "State_list=np.identity(Length_state)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "Epsilon_start=1\n",
    "Epsilon_final=0.01\n",
    "Decay_Rate=0.000001\n",
    "Velocity_tripod=0.289*40\n",
    "Velocity_quadruped=0.115*40\n",
    "Velocity_onebyone=0.096*40\n",
    "Up_degree=np.array([-40,-20,0,20,40])\n",
    "Left_degree=np.array([-60,-80,-100,-120])\n",
    "Right_degree=np.array([60,80,100,120]) #\n",
    "Robot_r=40 #the radius of hexapod robot\n",
    "Beta=0.9\n",
    "learning_rate=0.01\n",
    "Observe=1000\n",
    "Store_memory=deque()\n",
    "Memory_size=5000\n",
    "Batch_size=20\n",
    "Epoche=0\n",
    "Action_times=0"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "In the map, 'RGB' mode is used to reprensent different colors.\n",
    "\n",
    "(0,0,0)---Black---Obstacle and margin\n",
    "\n",
    "(255,255,255)---White---Flat ground\n",
    "\n",
    "(255,0,0)---Red---Destination\n",
    "\n",
    "(0,255,255)---Cyan---Hexapod robot"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def Draw_map1(Destination_x,Destination_y):\n",
    "    im1=Image.new('RGB',size=(3000,3000),color=(0,0,0))\n",
    "    draw=ImageDraw.Draw(im1,mode='RGB')\n",
    "    draw.rectangle((40,40,2960,2960),(128,138,135),(128,138,135))\n",
    "    draw.rectangle((500,800,1200,1200),(255,255,255),(255,255,255))\n",
    "    draw.rectangle((200,300,1500,700),(255,255,255),(255,255,255))\n",
    "    draw.rectangle((1800,300,2500,700),(255,255,255),(255,255,255))\n",
    "    draw.rectangle((2000,1200,2700,1900),(202,235,216),(202,235,216))\n",
    "    draw.rectangle((300,1600,1000,2700),(202,235,216),(202,235,216))\n",
    "    draw.rectangle((1600,2100,2500,2700),(255,255,255),(255,255,255))\n",
    "    draw.ellipse((Destination_x-20,Destination_y-20,Destination_x+20,Destination_y+20), (255,0,0),(255,0,0))\n",
    "    return im1\n",
    "# im1=Draw_map1(1750,500)\n",
    "# plt.imshow(im1)\n",
    "# plt.show()\n",
    "\n",
    "\n",
    "\n",
    "def Draw_map2(Destination_x,Destination_y):\n",
    "    im2=Image.new('RGB',size=(3000,3000),color=(0,0,0))\n",
    "    draw=ImageDraw.Draw(im2,mode='RGB')\n",
    "    draw.rectangle((40,40,2960,2960),(202,235,216),(202,235,216))\n",
    "    draw.rectangle((500,800,1200,1200),(128,138,135),(128,138,135))\n",
    "    draw.rectangle((200,300,1500,700),(255,255,255),(255,255,255))\n",
    "    draw.rectangle((1800,300,2500,700),(255,255,255),(255,255,255))\n",
    "    draw.rectangle((2000,1200,2700,1900),(128,138,135),(128,138,135))\n",
    "    draw.rectangle((300,1600,1000,2700),(128,138,135),(128,138,135))\n",
    "    draw.rectangle((1600,2100,2500,2700),(255,255,255),(255,255,255))\n",
    "    draw.ellipse((Destination_x-20,Destination_y-20,Destination_x+20,Destination_y+20), (255,0,0),(255,0,0))\n",
    "    return im2\n",
    "# im2=Draw_map2(1750,500)\n",
    "# plt.imshow(im2)\n",
    "# plt.show()\n",
    "\n",
    "\n",
    "def Draw_map3(Destination_x,Destination_y):\n",
    "    im3=Image.new('RGB',size=(3000,3000),color=(0,0,0))\n",
    "    draw=ImageDraw.Draw(im3,mode='RGB')\n",
    "    draw.rectangle((40,40,2960,2960),(202,235,216),(202,235,216))\n",
    "    draw.rectangle((500,800,1000,1200),(255,255,255),(255,255,255))\n",
    "    draw.rectangle((500,800,800,1200),(255,255,255),(255,255,255))\n",
    "    draw.rectangle((900,1300,1500,1800),(128,138,135),(128,138,135))\n",
    "    draw.rectangle((2000,800,800,1200),(255,255,255),(255,255,255))\n",
    "    draw.rectangle((500,250,800,500),(255,255,255),(255,255,255))\n",
    "    draw.rectangle((2500,200,2700,1000),(128,138,135),(128,138,135))\n",
    "    draw.rectangle((1100,300,2000,700),(128,138,135),(128,138,135))\n",
    "    draw.rectangle((2200,1550,2700,1850),(255,255,255),(255,255,255))\n",
    "    draw.rectangle((300,2000,1000,2700),(255,255,255),(255,255,255))\n",
    "    draw.rectangle((1900,1900,2800,2500),(128,138,135),(128,138,135))\n",
    "    draw.ellipse((Destination_x-20,Destination_y-20,Destination_x+20,Destination_y+20), (255,0,0),(255,0,0))\n",
    "    return im3\n",
    "# im3=Draw_map3(1750,500)\n",
    "# plt.imshow(im3)\n",
    "# plt.show()\n",
    "\n",
    "\n",
    "def Draw_map4(Destination_x,Destination_y):\n",
    "    im4=Image.new('RGB',size=(3000,3000),color=(0,0,0))\n",
    "    draw=ImageDraw.Draw(im4,mode='RGB')\n",
    "    draw.rectangle((40,40,2960,2960),(255,255,255),(255,255,255))\n",
    "    draw.rectangle((500,800,1000,1200),(202,235,216),(202,235,216))\n",
    "    draw.rectangle((500,800,800,1200),(202,235,216),(202,235,216))\n",
    "    draw.rectangle((900,1300,1500,1800),(128,138,135),(128,138,135))\n",
    "    draw.rectangle((2000,800,800,1200),(202,235,216),(202,235,216))\n",
    "    draw.rectangle((500,250,800,500),(202,235,216),(202,235,216))\n",
    "    draw.rectangle((2500,200,2700,1000),(128,138,135),(128,138,135))\n",
    "    draw.rectangle((1100,300,2000,700),(128,138,135),(128,138,135))\n",
    "    draw.rectangle((2200,1550,2700,1850),(202,235,216),(202,235,216))\n",
    "    draw.rectangle((300,2000,1000,2700),(202,235,216),(202,235,216))\n",
    "    draw.rectangle((1900,1900,2800,2500),(128,138,135),(128,138,135))\n",
    "    draw.ellipse((Destination_x-20,Destination_y-20,Destination_x+20,Destination_y+20), (255,0,0),(255,0,0))\n",
    "    return im4\n",
    "# im4=Draw_map4(1750,500)\n",
    "# plt.imshow(im4)\n",
    "# plt.show()\n",
    "\n",
    "\n",
    "def Draw_map5(Destination_x,Destination_y):\n",
    "    im5=Image.new('RGB',size=(3000,3000),color=(0,0,0))\n",
    "    draw=ImageDraw.Draw(im5,mode='RGB')\n",
    "    draw.rectangle((40,40,2960,2960),(255,255,255),(255,255,255))\n",
    "    draw.rectangle((500,800,1200,1200),(128,138,135),(128,138,135))\n",
    "    draw.rectangle((200,300,1500,700),(202,235,216),(202,235,216))\n",
    "    draw.rectangle((1800,300,2500,700),(202,235,216),(202,235,216))\n",
    "    draw.rectangle((2000,1200,2700,1900),(128,138,135),(128,138,135))\n",
    "    draw.rectangle((300,1600,1000,2700),(128,138,135),(128,138,135))\n",
    "    draw.rectangle((1600,2100,2500,2700),(202,235,216),(202,235,216))\n",
    "    draw.ellipse((Destination_x-20,Destination_y-20,Destination_x+20,Destination_y+20), (255,0,0),(255,0,0))\n",
    "    return im5\n",
    "# im5=Draw_map5(1750,500)\n",
    "# plt.imshow(im5)\n",
    "# plt.show()\n",
    "\n",
    "def Draw_map6(Destination_x,Destination_y):\n",
    "    im6=Image.new('RGB',size=(3000,3000),color=(0,0,0))\n",
    "    draw=ImageDraw.Draw(im6,mode='RGB')\n",
    "    draw.rectangle((40,40,2960,2960),(128,138,135),(128,138,135))\n",
    "    draw.rectangle((500,800,1000,1200),(255,255,255),(255,255,255))\n",
    "    draw.rectangle((500,800,800,1200),(255,255,255),(255,255,255))\n",
    "    draw.rectangle((900,1300,1500,1800),(202,235,216),(202,235,216))\n",
    "    draw.rectangle((2000,800,800,1200),(255,255,255),(255,255,255))\n",
    "    draw.rectangle((500,250,800,500),(255,255,255),(255,255,255))\n",
    "    draw.rectangle((2500,200,2700,1000),(202,235,216),(202,235,216))\n",
    "    draw.rectangle((1100,300,2000,700),(202,235,216),(202,235,216))\n",
    "    draw.rectangle((2200,1550,2700,1850),(255,255,255),(255,255,255))\n",
    "    draw.rectangle((300,2000,1000,2700),(255,255,255),(255,255,255))\n",
    "    draw.rectangle((1900,1900,2800,2500),(202,235,216),(202,235,216))\n",
    "    draw.ellipse((Destination_x-20,Destination_y-20,Destination_x+20,Destination_y+20), (255,0,0),(255,0,0))\n",
    "    return im6\n",
    "# im6=Draw_map6(1750,500)\n",
    "# plt.imshow(im6)\n",
    "# plt.show()\n",
    "\n",
    "def Draw_map7(Destination_x,Destination_y):\n",
    "    im7=Image.new('RGB',size=(3000,3000),color=(0,0,0))\n",
    "    draw=ImageDraw.Draw(im7,mode='RGB')\n",
    "    draw.rectangle((40,40,2960,2960),(255,255,255),(255,255,255))\n",
    "    draw.ellipse((300,400,800,900),(128,138,135),(128,138,135))\n",
    "    draw.ellipse((1000,1000,2000,2000),(128,138,135),(128,138,135))\n",
    "    draw.rectangle((2200,2200,2800,2800),(202,235,216),(202,235,216))\n",
    "    draw.ellipse((1800,300,2700,1000),(202,235,216),(202,235,216))\n",
    "    draw.rectangle((200,2200,1500,2600),(202,235,216),(202,235,216))\n",
    "    draw.ellipse((Destination_x-20,Destination_y-20,Destination_x+20,Destination_y+20), (255,0,0),(255,0,0))\n",
    "    return im7\n",
    "# im7=Draw_map7(1750,500)\n",
    "# plt.imshow(im7)\n",
    "# plt.show()\n",
    "\n",
    "def Draw_map8(Destination_x,Destination_y):\n",
    "    im8=Image.new('RGB',size=(3000,3000),color=(0,0,0))\n",
    "    draw=ImageDraw.Draw(im8,mode='RGB')\n",
    "    draw.rectangle((40,40,2960,2960),(128,138,135),(128,138,135))\n",
    "    draw.ellipse((300,400,800,900),(255,255,255),(255,255,255))\n",
    "    draw.ellipse((1000,1000,2000,2000),(255,255,255),(255,255,255))\n",
    "    draw.rectangle((2200,2200,2800,2800),(202,235,216),(202,235,216))\n",
    "    draw.ellipse((1800,300,2700,1000),(202,235,216),(202,235,216))\n",
    "    draw.rectangle((200,2200,1500,2600),(202,235,216),(202,235,216))\n",
    "    draw.ellipse((Destination_x-20,Destination_y-20,Destination_x+20,Destination_y+20), (255,0,0),(255,0,0))\n",
    "    return im8\n",
    "# im8=Draw_map8(1750,500)\n",
    "# plt.imshow(im8)\n",
    "# plt.show()\n",
    "\n",
    "def Draw_map9(Destination_x,Destination_y):\n",
    "    im9=Image.new('RGB',size=(3000,3000),color=(0,0,0))\n",
    "    draw=ImageDraw.Draw(im9,mode='RGB')\n",
    "    draw.rectangle((40,40,2960,2960),(202,235,216),(202,235,216))\n",
    "    draw.ellipse((300,400,800,900),(255,255,255),(255,255,255))\n",
    "    draw.ellipse((1000,1000,2000,2000),(255,255,255),(255,255,255))\n",
    "    draw.rectangle((2200,2200,2800,2800),(128,138,135),(128,138,135))\n",
    "    draw.ellipse((1800,300,2700,1000),(128,138,135),(128,138,135))\n",
    "    draw.rectangle((200,2200,1500,2600),(128,138,135),(128,138,135))\n",
    "    draw.ellipse((Destination_x-20,Destination_y-20,Destination_x+20,Destination_y+20), (255,0,0),(255,0,0))\n",
    "    return im9\n",
    "# im9=Draw_map9(1750,500)\n",
    "# plt.imshow(im9)\n",
    "# plt.show()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "Learning_rate=0.0001\n",
    "State_input=tf.placeholder(tf.float32,[None,Length_state])\n",
    "Action_input=tf.placeholder(tf.float32,[None,Length_action])\n",
    "Q_target_output=tf.placeholder(tf.float32,[None])\n",
    "\n",
    "Layer1_number=30\n",
    "w1=tf.Variable((tf.random_normal([Length_state,Layer1_number])))*0.01\n",
    "b1=tf.Variable(tf.zeros([1,Layer1_number]))\n",
    "l1=tf.nn.relu(tf.matmul(State_input,w1))+b1\n",
    "\n",
    "Layer12_number=20\n",
    "w12=tf.Variable((tf.random_normal([Layer1_number,Layer12_number])))*0.01\n",
    "b12=tf.Variable(tf.zeros([1,Layer12_number]))\n",
    "l12=tf.nn.relu(tf.matmul(l1,w12))+b12\n",
    "\n",
    "Layer2_number=Length_action\n",
    "w2=tf.Variable((tf.random_normal([Layer12_number,Layer2_number])))*0.01\n",
    "b2=tf.Variable(tf.zeros([1,Layer2_number]))\n",
    "l2=tf.matmul(l12,w2)+b2\n",
    "\n",
    "\n",
    "Q_value=tf.reduce_sum(tf.multiply(l2,Action_input),reduction_indices=1)\n",
    "Loss=tf.reduce_mean(tf.square(Q_value-Q_target_output))\n",
    "Optimizer=tf.train.AdamOptimizer(learning_rate).minimize(Loss)\n",
    "Predict=tf.argmax(l2,1)\n",
    "\n",
    "session=tf.InteractiveSession()\n",
    "session.run(tf.global_variables_initializer())\n",
    "\n",
    "def Save_memory(Current_state,Next_action,Next_state,Reward,Arrive):\n",
    "    Current_state_range=State_list[Current_state:Current_state+1]\n",
    "    Next_action_range=Action_list[Next_action:Next_action+1]\n",
    "    Next_state_range=State_list[Next_state:Next_state+1]\n",
    "    Store_memory.append((Current_state_range,Next_action_range,Next_state_range,Reward,Arrive))\n",
    "    if len(Store_memory)>Memory_size:\n",
    "        Store_memory.popleft()\n",
    "        \n",
    "def Experience_replay():\n",
    "    Minibatch=random.sample(Store_memory,Batch_size)\n",
    "    Batch_current_state = None\n",
    "    Batch_next_action = None\n",
    "    Batch_reward = None\n",
    "    Batch_next_state = None\n",
    "    Batch_arrive = None\n",
    "    \n",
    "    for index in range(Batch_size):\n",
    "        if Batch_current_state is None:\n",
    "            Batch_current_state=Minibatch[index][0]\n",
    "        elif Batch_current_state is not None:\n",
    "            Batch_current_state=np.vstack((Batch_current_state,Minibatch[index][0]))\n",
    "        \n",
    "        if Batch_next_action is None:\n",
    "            Batch_next_action=Minibatch[index][1]\n",
    "        elif Batch_next_action is not None:\n",
    "            Batch_next_action=np.vstack((Batch_next_action,Minibatch[index][1]))\n",
    "    \n",
    "        \n",
    "        if Batch_next_state is None:\n",
    "            Batch_next_state=Minibatch[index][2]\n",
    "        elif Batch_next_state is not None:\n",
    "            Batch_next_state=np.vstack((Batch_next_state,Minibatch[index][2]))\n",
    "        \n",
    "        if Batch_reward is None:\n",
    "            Batch_reward=Minibatch[index][3]\n",
    "        elif Batch_reward is not None:\n",
    "            Batch_reward=np.vstack((Batch_reward,Minibatch[index][3]))\n",
    "    \n",
    "        if Batch_arrive is None:\n",
    "            Batch_arrive=Minibatch[index][4]\n",
    "        elif Batch_arrive is not None:\n",
    "            Batch_arrive=np.vstack((Batch_arrive,Minibatch[index][4]))\n",
    "            \n",
    "    Q_next=session.run(l2,feed_dict={State_input:Batch_next_state})\n",
    "    Q_target_list=[]\n",
    "    for i in range(Batch_size):\n",
    "        Each_reward=Batch_reward[i][0]\n",
    "        Each_qvalue=Each_reward+Beta*np.max(Q_next[i])\n",
    "        if Each_reward<0:\n",
    "            Q_target_list.append(Each_reward)\n",
    "        else:\n",
    "            Q_target_list.append(Each_qvalue)\n",
    "\n",
    "    _,Cost,Reward=session.run([Q_value,Loss,Optimizer],feed_dict={State_input:Batch_current_state,\n",
    "                                                                         Action_input:Batch_next_action,\n",
    "                                                                         Q_target_output:Q_target_list})\n",
    "    if Action_times%5000==0:\n",
    "        print('Action_times=%d  Loss=%f'%(Action_times,Cost))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Set_destination(x_range,y_range):\n",
    "    x=np.random.randint(300,x_range-300)\n",
    "    y=np.random.randint(300,y_range-300)\n",
    "    return int(x),int(y)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Robot_destination(Destination_x,Destination_y,Robot_x,Robot_y,Robot_a):\n",
    "    Delta_x=Destination_x-Robot_x\n",
    "    Delta_y=Destination_y-Robot_y\n",
    "    Distance=np.sqrt(Delta_x*Delta_x+Delta_y*Delta_y)\n",
    "    Direct_angle=(np.arctan(Delta_y/Delta_x))*180/np.pi\n",
    "    if Delta_x<0:\n",
    "        Direct_angle=Direct_angle+180\n",
    "    True_angle=(Robot_a-Direct_angle)%360\n",
    "    if True_angle>180:\n",
    "        return 360-True_angle,'right',Distance\n",
    "    else:\n",
    "        return True_angle,'left',Distance"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Is_Crash(Current_x,Current_y,im):\n",
    "    Crash=False\n",
    "    Degree=[-150,-120,-90,-60,-30,0,30,60,90,120,150,180]\n",
    "    Dis=np.arange(0,50,5)\n",
    "    for i in Dis:\n",
    "        for j in Degree:\n",
    "            x=Current_x+i*np.cos(j/180*np.pi)\n",
    "            y=Current_y+i*np.sin(j/180*np.pi)\n",
    "            if (im.getpixel((x,y)))==(0,0,0):\n",
    "                Crash=True\n",
    "                break\n",
    "        if Crash==True:\n",
    "                break\n",
    "    return Crash\n",
    "# Is_Crash(10,10,im4)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Is_arrive(Current_x,Current_y,im):\n",
    "    Arrive=False\n",
    "    Crash=False\n",
    "    Degree=[-150,-120,-90,-60,-30,0,30,60,90,120,150,180]\n",
    "    Dis=np.arange(0,30,5)\n",
    "    for i in Dis:\n",
    "        for j in Degree:\n",
    "            x=Current_x+i*np.cos(j/180*np.pi)\n",
    "            y=Current_y+i*np.sin(j/180*np.pi)\n",
    "            if (im.getpixel((x,y)))==(0,0,0):\n",
    "                Crash=True\n",
    "                break\n",
    "            if (im.getpixel((x,y)))==(255,0,0):\n",
    "                Arrive=True\n",
    "                break\n",
    "        if Arrive==True or Crash==True:\n",
    "                break\n",
    "    return Arrive\n",
    "# print(Is_arrive(1750,500,im4))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Random_start(x_range,y_range,im):\n",
    "    a=np.random.random()*360\n",
    "    x=np.random.random()*x_range\n",
    "    y=np.random.random()*y_range\n",
    "    while(Is_Crash(x,y,im)==True or Is_arrive(x,y,im)==True):\n",
    "        x=np.random.random()*x_range\n",
    "        y=np.random.random()*y_range\n",
    "    return x,y,a\n",
    "  "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Height_level(Current_x,Current_y,Current_a,im):\n",
    "    Degree=[-30,0,30]\n",
    "    Distance=np.arange(0,30,5)\n",
    "    Max_height=0\n",
    "    for i in Degree:\n",
    "        for j in Distance:\n",
    "            x=Current_x+i*np.cos((j+Current_a)/180*np.pi)\n",
    "            y=Current_y+i*np.sin((j+Current_a)/180*np.pi)\n",
    "            Height_level=0\n",
    "            if im.getpixel((x,y))==(255,255,255):\n",
    "                Height_level=0\n",
    "            elif im.getpixel((x,y))==(202,235,216):\n",
    "                Height_level=1\n",
    "            elif im.getpixel((x,y))==(128,138,135):\n",
    "                Height_level=2\n",
    "            Max_height=max(Height_level,Max_height)\n",
    "            if Max_height==2:\n",
    "                break\n",
    "        if Max_height==2:\n",
    "            break\n",
    "    return Height_level\n",
    "# Height_level(750,1000,im4)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Output_state_index(True_angle,Left_right,Distance,Height):\n",
    "    if 0<=Distance<60:\n",
    "        Dis_level=0\n",
    "    elif 60<=Distance<180:\n",
    "        Dis_level=1\n",
    "    elif 180<=Distance<250:\n",
    "        Dis_level=2\n",
    "    else:\n",
    "        Dis_level=3\n",
    "    Angle_level=True_angle//30\n",
    "    if Left_right=='right':\n",
    "        Le_ri=1\n",
    "        State_number=int(Height*48+Dis_level+24*Le_ri+(5-Angle_level)*4)\n",
    "    elif Left_right=='left':\n",
    "        Le_ri=0\n",
    "        Angle_level=True_angle//30\n",
    "        State_number=int(Height*48+Dis_level+24*Le_ri+Angle_level*4)\n",
    "    return State_number,Dis_level,Angle_level\n",
    "# A,B,C=Output_state_index(80,'right',110,1)\n",
    "# States[A]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Choose_action(Current_state,Action_times):\n",
    "    Current_state_range=State_list[Current_state:Current_state+1]\n",
    "    Epsilon=Epsilon_final+(Epsilon_start-Epsilon_final)*np.exp(-1*Decay_Rate*Action_times)\n",
    "    if(np.random.random()<Epsilon):\n",
    "        Next_action=np.random.randint(Length_action)\n",
    "    else:\n",
    "        Action_qvalue_output=session.run(l2,feed_dict={State_input:Current_state_range})\n",
    "        Next_action=np.argmax(Action_qvalue_output)\n",
    "        \n",
    "    if Action_times%5000==0:\n",
    "        print('Action_times=%d  Epsilon=%f Epoche=%d'%(Action_times,Epsilon,Epoche))\n",
    "    return Next_action"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Next_state_F(Robot_x,Robot_y,Robot_a,Destination_x,Destination_y,Action,im):\n",
    "    Arrive=False\n",
    "    Crash=False\n",
    "    Current_height=Height_level(Robot_x,Robot_y,Robot_a,im)\n",
    "    \n",
    "    if Action==0:\n",
    "        Robot_next_x=Robot_x+Velocity_tripod*np.cos(Robot_a/180*np.pi)\n",
    "        Robot_next_y=Robot_y+Velocity_tripod*np.sin(Robot_a/180*np.pi)\n",
    "        if Is_Crash(Robot_next_x,Robot_next_y,im)==True:\n",
    "            Crash=True\n",
    "        if Is_arrive(Robot_next_x,Robot_next_y,im)==True:\n",
    "            Arrive=True\n",
    "    elif Action==1:\n",
    "        Robot_next_x=Robot_x+Velocity_quadruped*np.cos(Robot_a/180*np.pi)\n",
    "        Robot_next_y=Robot_y+Velocity_quadruped*np.sin(Robot_a/180*np.pi)\n",
    "        if Is_Crash(Robot_next_x,Robot_next_y,im)==True:\n",
    "            Crash=True\n",
    "        if Is_arrive(Robot_next_x,Robot_next_y,im)==True:\n",
    "            Arrive=True\n",
    "    elif Action==2:\n",
    "        Robot_next_x=Robot_x+Velocity_onebyone*np.cos(Robot_a/180*np.pi)\n",
    "        Robot_next_y=Robot_y+Velocity_onebyone*np.sin(Robot_a/180*np.pi)\n",
    "        if Is_Crash(Robot_next_x,Robot_next_y,im)==True:\n",
    "            Crash=True\n",
    "        if Is_arrive(Robot_next_x,Robot_next_y,im)==True:\n",
    "            Arrive=True\n",
    "    elif Action==3:\n",
    "        Robot_a=Robot_a-60\n",
    "        Robot_next_x=Robot_x\n",
    "        Robot_next_y=Robot_y\n",
    "    elif Action==4:\n",
    "        Robot_a=Robot_a+60\n",
    "        Robot_next_x=Robot_x\n",
    "        Robot_next_y=Robot_y\n",
    "        \n",
    "   \n",
    "    \n",
    "    Next_true_angle,Next_le_ri,Next_distance=Robot_destination(Destination_x,Destination_y,Robot_next_x,Robot_next_y,Robot_a)\n",
    "    Next_height=Height_level(Robot_next_x,Robot_next_y,Robot_a,im)\n",
    "    Next_state_number,Next_distance_level,Next_angle_level=Output_state_index(Next_true_angle,Next_le_ri,Next_distance,Next_height)\n",
    "    \n",
    "    if (Current_height==2 and Action==0) or (Current_height==2 and Action==1):\n",
    "        Reward=-2\n",
    "    elif(Current_height==1 and Action==0) or (Current_height==1 and Action==2):\n",
    "        Reward=-2\n",
    "    elif(Current_height==0 and Action==1) or (Current_height==0 and Action==2):\n",
    "        Reward=-2\n",
    "    else:\n",
    "        if Arrive==False:\n",
    "            Reward=(-Next_distance_level-Next_angle_level*0.5)*0.01\n",
    "        else:\n",
    "            Reward=2\n",
    "    return Robot_next_x,Robot_next_y,Robot_a,Reward,Next_state_number,Arrive,Crash"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "collapsed": true
   },
   "outputs": [],
   "source": [
    "def Run_function():\n",
    "    global Epoche\n",
    "    global Action_times\n",
    "    global Vector_x\n",
    "    global Vector_y\n",
    "    Crash_=False\n",
    "    Arrive_=False\n",
    "    Destination_x,Destination_y=Set_destination(3000,3000)\n",
    "    Choose_map=np.random.randint(9)\n",
    "    if Choose_map==0:\n",
    "        im=Draw_map1(Destination_x,Destination_y)\n",
    "    elif Choose_map==1:\n",
    "        im=Draw_map2(Destination_x,Destination_y)\n",
    "    elif Choose_map==2:\n",
    "        im=Draw_map3(Destination_x,Destination_y)\n",
    "    elif Choose_map==3:\n",
    "        im=Draw_map4(Destination_x,Destination_y)\n",
    "    elif Choose_map==4:\n",
    "        im=Draw_map5(Destination_x,Destination_y)\n",
    "    elif Choose_map==5:\n",
    "        im=Draw_map6(Destination_x,Destination_y)\n",
    "    elif Choose_map==6:\n",
    "        im=Draw_map7(Destination_x,Destination_y)\n",
    "    elif Choose_map==7:\n",
    "        im=Draw_map8(Destination_x,Destination_y)\n",
    "    else:\n",
    "        im=Draw_map9(Destination_x,Destination_y)\n",
    "    Current_x,Current_y,Current_a=Random_start(3000,3000,im)\n",
    "    Epoche_false=0\n",
    "    while Epoche_false<5000:\n",
    "        True_a,Le_ri,Distance=Robot_destination(Destination_x,Destination_y,Current_x,Current_y,Current_a)\n",
    "        Height_level_=Height_level(Current_x,Current_y,Current_a,im)\n",
    "        Current_state_number,Distance_level,Angle_level=Output_state_index(True_a,Le_ri,Distance,Height_level_)\n",
    "        Next_action=Choose_action(Current_state_number,Action_times)\n",
    "        Next_x,Next_y,Next_a,Reward,Next_state_number,Arrive,Crash=Next_state_F(Current_x,Current_y,Current_a,Destination_x,Destination_y,Next_action,im)\n",
    "        if Crash==True:\n",
    "            Next_x,Next_y,Next_a=Random_start(3000,3000,im)\n",
    "        else:\n",
    "            Save_memory(Current_state_number,Next_action,Next_state_number,Reward,Arrive)\n",
    "            if Action_times>Observe:\n",
    "                Experience_replay()\n",
    "            if Arrive:\n",
    "                Destination_x,Destination_y=Set_destination(3000,3000)\n",
    "                Choose_map=np.random.randint(9)\n",
    "                if Choose_map==0:\n",
    "                    im=Draw_map1(Destination_x,Destination_y)\n",
    "                elif Choose_map==1:\n",
    "                    im=Draw_map2(Destination_x,Destination_y)\n",
    "                elif Choose_map==2:\n",
    "                    im=Draw_map3(Destination_x,Destination_y)\n",
    "                elif Choose_map==3:\n",
    "                    im=Draw_map4(Destination_x,Destination_y)\n",
    "                elif Choose_map==4:\n",
    "                    im=Draw_map5(Destination_x,Destination_y)\n",
    "                elif Choose_map==5:\n",
    "                    im=Draw_map6(Destination_x,Destination_y)\n",
    "                elif Choose_map==6:\n",
    "                    im=Draw_map7(Destination_x,Destination_y)\n",
    "                elif Choose_map==7:\n",
    "                    im=Draw_map8(Destination_x,Destination_y)\n",
    "                else:\n",
    "                    im=Draw_map9(Destination_x,Destination_y)\n",
    "                Next_x,Next_y,Next_a=Random_start(3000,3000,im)\n",
    "                Epoche_false+=1\n",
    "                Epoche+=1\n",
    "        Current_x=Next_x\n",
    "        Current_y=Next_y\n",
    "        Current_a=Next_a\n",
    "        Action_times+=1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "Run_function()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "After_Q=session.run(l2,feed_dict={State_input:State_list})\n",
    "Max_qaction=np.argmax(After_Q,axis=1)\n",
    "Show_table=pd.DataFrame([States,Max_qaction]).T\n",
    "Show_table[Show_table==0]='FrontH0'\n",
    "Show_table[Show_table==1]='FrontH1'\n",
    "Show_table[Show_table==2]='FrontH2'\n",
    "Show_table[Show_table==3]='left'\n",
    "Show_table[Show_table==4]='right'\n",
    "Show_table\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "scrolled": false
   },
   "outputs": [],
   "source": [
    "# f=lambda x:np.argmax(np.bincount(x))\n",
    "# Max0=np.loadtxt(\"Test1_MaxQValueAction0.txt\")\n",
    "# Max1=np.loadtxt(\"Test1_MaxQValueAction1.txt\")\n",
    "# Max2=np.loadtxt(\"Test1_MaxQValueAction2.txt\")\n",
    "# Max3=np.loadtxt(\"Test2_MaxQValueAction0.txt\")\n",
    "# Max4=np.loadtxt(\"Test2_MaxQValueAction1.txt\")\n",
    "# Max5=np.loadtxt(\"Test3_MaxQValueAction0.txt\")\n",
    "# Max6=np.loadtxt(\"Test3_MaxQValueAction1.txt\")\n",
    "# Max7=np.loadtxt(\"Test2_MaxQValueAction2.txt\")\n",
    "# Max8=np.loadtxt(\"Test3_MaxQValueAction2.txt\")\n",
    "# Max9=np.loadtxt(\"Test3_MaxQValueAction3.txt\")\n",
    "# # Max10=np.loadtxt(\"Test3_MaxQValueAction4.txt\")\n",
    "# Max11=np.loadtxt(\"Test3_MaxQValueAction5.txt\")\n",
    "# Max12=np.loadtxt(\"Test3_MaxQValueAction6.txt\")\n",
    "# Max13=np.loadtxt(\"Test3_MaxQValueAction8.txt\")\n",
    "# Max14=np.loadtxt(\"Test3_MaxQValueAction9.txt\")\n",
    "# Max15=np.loadtxt(\"Test3_MaxQValueAction11.txt\")\n",
    "# Max=pd.DataFrame([Max0,Max1,Max2,Max3,Max4,Max5,Max6,Max7,Max8,Max9,Max11,Max12,Max13,Max14,Max15],dtype=np.int8).T\n",
    "# Max.columns=['Max0','Max1','Max2','Max3','Max4','Max5','Max6','Max7','Max8','Max9','Max11','Max12','Max13','Max14','Max15']\n",
    "# Max.index=States\n",
    "# Max['Final_action']=Max.apply(f,axis=1)\n",
    "# Max"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
